package nezumi.state;

import java.awt.Point;
import java.awt.geom.Point2D;
import java.io.IOException;
import java.util.Observable;
import java.util.Observer;
import java.util.logging.Logger;

import javax.xml.parsers.DocumentBuilder;
import javax.xml.parsers.DocumentBuilderFactory;
import javax.xml.parsers.ParserConfigurationException;

import nezumi.Constants;
import nezumi.Nezumi;
import nezumi.planning.Planner;
import nezumi.sensing.BeaconDiscreteSensorModel;
import nezumi.sensing.ISensorModel;
import nezumi.util.AngleGeometry;
import nezumi.util.XMLUtils;

import org.w3c.dom.Document;
import org.w3c.dom.Node;
import org.xml.sax.SAXException;

public class TargetState implements Observer {

	private static final int MIN_SIGHTINGS = 4;
	private static final ISensorModel beaconSensorModel = new BeaconDiscreteSensorModel();
	protected static final Logger logger = Logger.getLogger(TargetState.class
			.getName());
	public static final int NO_TARGET = -1;

	// we store the target position's likelihood as a 3d matrix
	private double targetGrid[][][];
	private double targetSightings[];
	private final Point2D likelyTargets[];
	private boolean isTargetVisited[];
	private final int width, height, ntargets;
	private State state;

	public TargetState(final State state, final int width, final int height,
			final int ntargets) {
		assert state != null : "state = " + state;
		assert width > 0 : "width = " + width;
		assert height > 0 : "height = " + height;
		assert ntargets > 0 : "ntargets = " + ntargets;

		this.state = state;
		this.width = width;
		this.height = height;
		this.ntargets = ntargets;

		// zero sightings in the beginning
		targetSightings = new double[ntargets];
		isTargetVisited = new boolean[ntargets];

		// set the target to the middle of the maze at the beginning
		likelyTargets = new Point2D[ntargets];
		for (int i = 0; i < ntargets; i++)
			likelyTargets[i] = new Point2D.Double(width / 2 + 1, height / 2 + 1);

		// initialize the target grid to zero
		targetGrid = new double[ntargets][width][height];
		this.clearTargetGrid();
	}

	/* getters & setters - START */

	public double[][][] getTargetGrid() {
		return targetGrid;
	}

	public double[] getTargetSightings() {
		return targetSightings;
	}

	public Point2D[] getLikelyTargets() {
		return likelyTargets;
	}

	/* getters & setters - END */

	public boolean isAllVisited() {
		for (boolean visited : isTargetVisited)
			if (!visited)
				return false;
		return true;
	}

	public boolean isVisited(final int targetId) {
		assert targetId != TargetState.NO_TARGET && targetId >= 0
				&& targetId < ntargets : "targetId = " + targetId;

		return isTargetVisited[targetId];
	}

	/**
	 * Sets all cells to zero.
	 */
	public void clearTargetGrid() {

		for (int i = 0; i < ntargets; i++)
			for (int j = 0; j < width; j++)
				for (int k = 0; k < height; k++)
					targetGrid[i][j][k] = 0.0;
	}

	public void updateTargetGrid(final int idx) {

		assert idx >= 0 && idx < ntargets : "idx" + idx;

		final AgentState astate = this.state.getAgentState();
		final Point2D pos = astate.getHistoricPosition();
		final double baseOrientation = astate.getHistoricOrientation();

		// guard for the cases when we still don't have enough history to
		// make
		// sense of the beacon data
		if (pos == null || baseOrientation == Double.NaN)
			return;

		final double beaconRot = baseOrientation
				+ Math.toRadians(this.state.getSensorState().getSensor(
						SensorState.BEACON + idx));

		for (int i = 0; i < this.width; i++)
			for (int j = 0; j < height; j++) {
				final double centeri = i + (Constants.MAP_GRANULARITY / 2), centerj = j
						+ (Constants.MAP_GRANULARITY / 2);

				final double pointRot = Math.atan2(centerj - pos.getY(),
						centeri - pos.getX());

				if (Math
						.abs(AngleGeometry.angleDifference(beaconRot, pointRot)) < Math
						.toRadians(Constants.BEACON_STD_DEV)) {
					targetGrid[idx][(int) i][(int) j] += beaconSensorModel
							.computeProb(beaconRot, pointRot);
					if (targetGrid[idx][(int) i][(int) j] >= this.targetSightings[idx]) {
						this.targetSightings[idx] = targetGrid[idx][(int) i][(int) j];
						this.likelyTargets[idx].setLocation(i, j);

						// if this is the target we are looking for at the
						// moment we need to update the plan
						Planner planner = this.state.getPlanner();
						if (planner.getTargetId() == idx
								&& !planner.isGoingHome())
							planner.setCurrTarget(new Point(
									(int) this.likelyTargets[idx].getX(),
									(int) this.likelyTargets[idx].getY()));
					}
				}
			}
	}

	/**
	 * Loads the target's positions form an XML specification files.
	 */
	public void loadTargetState(final String labfile, final String gridfile)
			throws SAXException, IOException {

		assert labfile != null : "labfile = " + labfile;

		// factory design pattern
		DocumentBuilderFactory fac = DocumentBuilderFactory.newInstance();
		DocumentBuilder db = null;
		try {
			db = fac.newDocumentBuilder();
		} catch (ParserConfigurationException e) {
			logger
					.severe(String
							.format(
									"Error creating %s for loading a map from an XML labyrinth file. Exception: %s",
									db.getClass().getName(), e.getMessage()));
			return;
		}

		// parse the grid file
		Document doc = db.parse(gridfile);

		// we are only interested in the first target for now
		final Node startNode = doc.getElementsByTagName("Position").item(0);

		// parse the labyrinth file
		doc = db.parse(labfile);

		// get the target
		// we are only interested in the first target for now
		for (int i = 0; i < ntargets; i++) {
			final Node targetNode = doc.getElementsByTagName("Target").item(0);

			final double target_x = XMLUtils.readDoubleAttribute(targetNode,
					"X")
					/ Constants.MAP_GRANULARITY;
			final double target_y = XMLUtils.readDoubleAttribute(targetNode,
					"Y")
					/ Constants.MAP_GRANULARITY;

			final Point2D startPos = this.state.getAgentState().getStartPos();
			double deltax = startPos.getX()
					- (Double.parseDouble(startNode.getAttributes()
							.getNamedItem("X").getTextContent()) / Constants.MAP_GRANULARITY);
			double deltay = startPos.getY()
					- (Double.parseDouble(startNode.getAttributes()
							.getNamedItem("Y").getTextContent()) / Constants.MAP_GRANULARITY);

			// set the target where it belongs
			this.likelyTargets[i].setLocation(target_x + deltax, target_y
					+ deltay);
			// make sure we have enough virtual sightings
			this.targetSightings[i] = MIN_SIGHTINGS;
		}

		// now we have a new target so we need to recompute the path
		this.state.getPlanner().plan();
	}

	/**
	 * We should be notified every time new sensor values come along.
	 */
	public void update(Observable o, Object arg) {
		if (arg instanceof Integer)
			this.updateTargetGrid(((Integer) arg).intValue());
		else if (arg instanceof String
				&& ((String) arg).equals(SensorState.GROUND))
			this.setCurrentTargetVisited();

	}

	private void setCurrentTargetVisited() {

		int targetId = this.state.getPlanner().getTargetId();

		assert targetId != TargetState.NO_TARGET && targetId >= 0
				&& targetId < ntargets : "targetId = " + targetId;

		isTargetVisited[targetId] = true;
		
		logger.warning(String.format("Setting led for beacon %d\n", targetId));
		Nezumi.cif.SetVisitingLed(true);
		Nezumi.led = true;
		
	}

	/**
	 * If a target was seen a minimum number of times.
	 * 
	 * @return
	 */
	public int isTargetSeen() {

		double max = 0;
		int idx = 0;

		for (int i = 0; i < ntargets; i++)
			if (!isTargetVisited[i] && targetSightings[i] > max) {
				max = targetSightings[i];
				idx = i;
			}

		return max >= MIN_SIGHTINGS ? idx : NO_TARGET;
	}
}